#include "bsp_sg90.h"

static uint16_t AngleToPulse(float angle)
{
    if (angle < 0)
        angle = 0;
    if (angle > 180)
        angle = 180;
    return (uint16_t)(MIN_PULSE + (angle / 180.0f) * PULSE_RANGE);
}

void SG90_SetAngle_CH1(float angle)
{
    uint16_t duty = AngleToPulse(angle);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, duty);
}

void SG90_SetAngle_CH2(float angle)
{
    uint16_t duty = AngleToPulse(angle);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, duty);
}

void SG90_SetAngle_CH3(float angle)
{
    uint16_t duty = AngleToPulse(angle);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, duty);
}

void SG90_SetAngle_CH4(float angle)
{
    uint16_t duty = AngleToPulse(angle);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, duty);
}

#ifndef SMOOTH_ALGORITHM
void Servo_Set_Target_Angle(uint8_t servo_id, float angle)
{
    if (servo_id == 0)
    {
        SG90_SetAngle_CH1(angle);
    }
    else if (servo_id == 1)
    {
        SG90_SetAngle_CH2(angle);
    }
    else if (servo_id == 2)
    {
        SG90_SetAngle_CH3(angle);
    }
    else if (servo_id == 3)
    {
        SG90_SetAngle_CH4(angle);
    }
}

// 下面代码实现平滑算法，比较难不要给实训学生讲
#else
Servo servos[NUM_SERVOS]; // 4个舵机数组

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM7)
    {
        // 平滑控制每个舵机的角度
        for (int i = 0; i < NUM_SERVOS; i++)
        {
            if (servos[i].current_angle < servos[i].target_angle)
            {
                servos[i].current_angle += 1; // 逐步增加角度
            }
            else if (servos[i].current_angle > servos[i].target_angle)
            {
                servos[i].current_angle -= 1; // 逐步减少角度
            }

            // 计算PWM脉冲宽度
            float angle = (float)servos[i].current_angle;

            // 设置对应通道的PWM脉冲宽度
            switch (i)
            {
            case 0:
                SG90_SetAngle_CH1(angle);
                break;
            case 1:
                SG90_SetAngle_CH2(angle);
                break;
            case 2:
                SG90_SetAngle_CH3(angle);
                break;
            case 3:
                SG90_SetAngle_CH4(angle);
                break;
            }
        }
    }
}

// 使用平滑算法，舵机的操作逻辑是不一样的，这个是依赖定时器来实现角度渐变
void Servo_Set_Target_Angle(uint8_t servo_id, float angle)
{
    if (angle > 180)
        angle = 180; // 限制角度范围
    servos[servo_id].target_angle = angle;
}
#endif